/* ######################################################################### */
/** \file ObjectDetection.hpp
 * \brief	This file contains the function that
 *
 *
 * PROJECT:   DATMO ROS NODE
 *
 * Copyright (c) 2011 CAR - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2011-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ         2011-06-23 \n
 *      First .
 *
 ######################################################################### */

#include "ObjectFinder.hpp"
#include "ObjectFinder2.hpp"
#include <math.h>
/* ------------------------------------------------------------------------- */
/**	\namespace DATMO
 *
 *  \brief	This is the namespace of the DATMO ROS NODE
 */
/* ------------------------------------------------------------------------- */

namespace DATMO
{
/* ------------------------------------------------------------------------- */
/**	\class ObjectDetection
 *
 *  \brief  This is the class of ObjectDetection function.
 *
 *
 */
/* ------------------------------------------------------------------------- */
class cl_ObjectDetection
{
public:
	/* ------------------------------------------------------------------------- */
	/**	\fn public static void ObjectDetection()
	 *
	 *  \brief
	 *
	 *  \param[in]
	 *  \param[in]
	 *
	 *  \param[out] bool
	 *
	 */
	/* ------------------------------------------------------------------------- */
	static DetectedObjects ObjectDetection(DistanceImage str_DistImage, DistanceImage str_LastDistImage)
	{
		/*Variables*/
		DynObjConst str_DynObjConst = DynObjConst();
		DetectedObjects str_DetectedObjects = DetectedObjects(str_DistImage.s_height, str_DistImage.s_width);
		Matrix tf_DistanceDiference = Matrix(str_DistImage.s_height, str_DistImage.s_width);
		Matrix tf_Objects = Matrix(str_DistImage.s_height, str_DistImage.s_width);
		float f_difference_tol = str_DynObjConst.f_laser_laser_dif_tol;
		float f_percentage_tol = str_DynObjConst.f_laser_scan_percentage_tol;
		int i_nzero = 0;  //number of elements in the difference that are zero
		/*Filter points with difference too small*/
		ROS_INFO("Entra en detection");
		for (int i = 1; i <= str_DistImage.s_height; i++)
		{
			for (int j = 1; j <= str_DistImage.s_width; j++)
			{
				tf_Objects(i,j) = 0.0f;
				tf_DistanceDiference(i,j) = str_DistImage.tf_Distance(i,j) - str_LastDistImage.tf_Distance(i,j);
				if (fabs(tf_DistanceDiference(i, j)) < f_difference_tol)
				{
					tf_DistanceDiference(i, j) = 0;
				}
				if (tf_DistanceDiference(i, j) == 0)
				{
					i_nzero++;
				}

			}
		}
		ROS_INFO("Primer bucle");
		if (i_nzero > f_percentage_tol * str_DistImage.s_height * str_DistImage.s_width)
		{
			str_DetectedObjects = cl_ObjectFinder2::ObjectFinder2(tf_DistanceDiference);
			str_DetectedObjects.i_status = 1;
		}

		return str_DetectedObjects;
	}


};
}

